#include <x86.h>
#include "elf.h"

#define SECT_SIZE 0x200
#define ELFHDR ((struct elf_t *) 0x10000)

void readsect(void*, uint32_t);
void readseg(uint32_t, uint32_t, uint32_t);

void
s1_main(void)
{
	 int stage15_sect_start =
		  (*(int *)(0x1000) + *(int *)(0x1004));

     struct proghdr_t *ph, *eph;

     // read 1st page off disk
	 // this limit the elf header of the kernel to be fit in 1 page
     readseg((uint32_t) ELFHDR, SECT_SIZE * 8, stage15_sect_start * SECT_SIZE);

     // is this a valid ELF?
     if (ELFHDR->e_magic != ELF_MAGIC)
		  goto bad;
     
     // load each program segment (ignores ph flags)
     ph = (struct proghdr_t *) ((uint8_t *) ELFHDR + ELFHDR->e_phoff);
     eph = ph + ELFHDR->e_phnum;
     for (; ph < eph; ph++)
     {
		  readseg(ph->p_va, ph->p_memsz,
				  stage15_sect_start * SECT_SIZE + ph->p_offset);
		  char *cur = ph->p_va - 0xF0000000, *end = cur + ph->p_memsz;
		  for (cur; cur != end; ++cur)
		  {
			   *cur = *(char *)((uint32_t)cur & 0xFFFFFF);
		  }
     }

     // call the entry point from the ELF header
     // note: does not return!

     // breakpoint();
     ((void (*)(void)) (ELFHDR->e_entry - 0xF0000000))();

bad:
     outw(0x8A00, 0x8A00);
     outw(0x8A00, 0x8E00);
     while (1)
		  /* do nothing */;
}

// Read 'count' bytes at 'offset' from kernel into virtual address 'va'.
// Might copy more than asked
void
readseg(uint32_t va, uint32_t count, uint32_t offset)
{
     uint32_t end_va;

     va &= 0xFFFFFF;
     end_va = va + count;
	
     // round down to sector boundary
     va &= ~(SECT_SIZE - 1);

     offset = (offset / SECT_SIZE);

     // If this is too slow, we could read lots of sectors at a time.
     // We'd write more to memory than asked, but it doesn't matter --
     // we load in increasing order.
     while (va < end_va) {
		  readsect((uint8_t*) va, offset);
		  va += SECT_SIZE;
		  offset++;
     }
}

void
waitdisk(void)
{
     // wait for disk ready
     while ((inb(0x1F7) & 0xC0) != 0x40)
		  /* do nothing */;
}

void
readsect(void *dst, uint32_t offset)
{
     // wait for disk to be ready
     waitdisk();

     outb(0x1F2, 1);		// count = 1
     outb(0x1F3, offset);
     outb(0x1F4, offset >> 8);
     outb(0x1F5, offset >> 16);
     outb(0x1F6, (offset >> 24) | 0xE0);
     outb(0x1F7, 0x20);	// cmd 0x20 - read sectors

     // wait for disk to be ready
     waitdisk();

     // read a sector
     insl(0x1F0, dst, SECT_SIZE/4);
}

